# pragma once 

# include "cpp_tools.h"
# include "dataStruct.h"

Eigen::VectorXd wheel_slip(double v, double beta, double omega, double delta, double R_wheel, double l_f, double l_r, double l_y)
{
    Eigen::VectorXd v_x_center(4);
    Eigen::VectorXd v_y_slip(4);

    v_x_center(0) = v*cos(beta - delta) + omega*l_f*sin(delta) - omega*l_y/2*cos(delta);// 左前
    v_x_center(1) = v*cos(beta - delta) + omega*l_f*sin(delta) + omega*l_y/2*cos(delta);// 右前
    v_x_center(2) = v*cos(beta - 0) - omega*l_r*sin(0) - omega*l_y/2*cos(0);// 左后
    v_x_center(3) = v*cos(beta - 0) - omega*l_r*sin(0) + omega*l_y/2*cos(0);// 右后

    v_y_slip(0) = -(v*sin(beta - delta) + omega*l_f*cos(delta) + omega*l_y/2*sin(delta));
    v_y_slip(1) = -(v*sin(beta - delta) + omega*l_f*cos(delta) - omega*l_y/2*sin(delta));
    v_y_slip(2) = -(v*sin(beta - 0) - omega*l_r*cos(0) + omega*l_y/2*sin(0));
    v_y_slip(3) = -(v*sin(beta - 0) - omega*l_r*cos(0) - omega*l_y/2*sin(0));

    Eigen::VectorXd s_y = v_y_slip.array()/(v_x_center.array());
    
    for (size_t i = 0; i < 4; i++)
    {
        if (v_x_center(i) == 0)
        {
            s_y(i) = 0;
        }
    }

    double max_s = 2;
    if (std::abs(v) < 1)
    {
        max_s = std::pow(v,2);
    }

    for (auto &&i : s_y)
    {
        i = cpp_tools::saturation(i,-max_s,max_s);
    }
    
    return s_y;
}


Eigen::VectorXd MF98(Eigen::VectorXd s_y, Eigen::VectorXd F_z, data::vehicle_param::tire::MF MF_y)
{
    Eigen::VectorXd Mu_y(4);
    for (size_t i = 0; i < 4; i++)
    {
        if (s_y(i) == 0)
        {
            Mu_y(i) = 0;
        }
        else
        {
            Mu_y(i) = MF_y.D*std::sin(MF_y.C*std::atan(s_y(i)*MF_y.B - MF_y.E*(s_y(i)*MF_y.B - std::atan(s_y(i)*MF_y.B))));
        }
    }

    return Mu_y.array()*F_z.array();
}